//#define _CRTDBG_MAP_ALLOC
#include "stdafx.h"
#include "filter_kernel.h"
#include <assert.h>


Filter_kernel::Filter_kernel(float sigmau, float sigmav, float phi, float omega)
{
	
	//assert( COLUMNS_BLOCKDIM_Y * COLUMNS_HALO_STEPS >= KERNEL_RADIUS );
	

	h_kernel_x   = (float *)malloc(KERNEL_SIZE_X);
    h_kernel_y   = (float *)malloc(KERNEL_SIZE_Y);
    h_kernel_t   = (float *)malloc(KERNEL_SIZE_T);

	omegax = (float)(omega * cos(phi) );
	/* flip sign in y because ij indexing not xy */
    omegay = (float)(-omega * sin(phi) );  

	float sigmax;
	float sigmay;
	float sigmat;
	
    float su2 = sigmau*sigmau;
    float sv2 = sigmav*sigmav;
	
	float theta = phi+PI/2;
    /* elements of covariance matrix */
	float c11 = su2*cos(theta)*cos(theta)+sv2*sin(theta)*sin(theta);
	float c22 = su2*sin(theta)*sin(theta)+sv2*cos(theta)*cos(theta);
	float c12 = (su2-sv2)*cos(theta)*sin(theta);

	/* standard deviations along different axes */
	sigmat = sqrt(abs(c12));

	/* choose diagonal axis */
	if(sigmat < 0.001)	
	{
        sigmax = sqrt(c11);
		sigmay = sqrt(c22);
		tanp =0.0;
    }
	else if (c12 > 0)
	{
        sigmax = sqrt(c11-c12);
		sigmay = sqrt(c22-c12);
		tanp =1.0;
    }
	else
	{
        sigmax = sqrt(c11+c12);
		sigmay = sqrt(c22+c12);
		tanp =-1.0;
	}

	assert(KERNEL_RADIUS_X > 3 * sigmax && KERNEL_RADIUS_X%16 == 0);
	assert(KERNEL_RADIUS_Y > 3 * sigmay && KERNEL_RADIUS_X%16 == 0);
	assert(KERNEL_RADIUS_T > 3 * sigmat && KERNEL_RADIUS_X%16 == 0);

        
	// calculate for the kernel for three axis
	float kernelSum = 0;
    for(int i = 0; i < KERNEL_W_X; i++){
        float dist = (float)(i - KERNEL_RADIUS_X) / (float)sigmax;
        h_kernel_x[i] = expf(- dist * dist / 2);
        kernelSum += h_kernel_x[i];
    }
    for(int i = 0; i < KERNEL_W_X; i++)
        h_kernel_x[i] /= kernelSum;
        
    kernelSum = 0;
    for(int i = 0; i < KERNEL_W_Y; i++){
        float dist = (float)(i - KERNEL_RADIUS_Y) / (float)sigmay;
        h_kernel_y[i] = expf(- dist * dist / 2);
        kernelSum += h_kernel_y[i];
    }
    for(int i = 0; i < KERNEL_W_Y; i++)
        h_kernel_y[i] /= kernelSum;
        
    kernelSum = 0;
    for(int i = 0; i < KERNEL_W_T; i++){
        float dist = (float)(i - KERNEL_RADIUS_T) / (float)sigmat;
        h_kernel_t[i] = expf(- dist * dist / 2);
        kernelSum += h_kernel_t[i];
    }        
    for(int i = 0; i < KERNEL_W_T; i++)
        h_kernel_t[i] /= kernelSum;


}


Filter_kernel::~Filter_kernel()
{	
	free(h_kernel_x);
	free(h_kernel_y);
	free(h_kernel_t);

}

